﻿#include "main.h"

void EPWM_IRQHandler(void)	            // 20KHz中断
{
    if(EPWM_GetZeroIntFlag(EPWM0))
    {
        // Motor_Open_Loop_Control(&Motor);
        //SC60228_Update();   //进行编码器的更新
        if((SSP->STAT & SSP_STAT_RNE_Msk)) 
        {
			SC60228_Update();
		}
		
        //if((!(SSP->STAT & SSP_STAT_BSY_Msk)) && ((SSP->STAT & SSP_STAT_TNF_Msk)))	SSP->DAT = 0x0000; 
		if(!(SSP->STAT & SSP_STAT_BSY_Msk))	SSP->DAT = 0x0000; 
        PLL_Speed_Run(SC60228_Data, &PLL_Angle, &PLL_Speed);
        Motor_Control();  
			
				
    }
		
    EPWM->ICLR = 0xFFFFFFFF;
}

// 

static uint16_t Phase_Ref[2] = {1062,1062 + 2048};//{1325,3078}; // {1108,3146}; //  对应按摩头上下两个位置时的编码器值 //3443

volatile uint16_t Position_Change_Time = 1000*4; // 调节击打频率
static uint8_t Ref_Index = 0;
static int16_t Motor_Position_Error = 0;
static float Position_P = 1.5f; // 对应打击力度，根据实际情况调试
static uint16_t Position_Change_Time_Count = 0;
static int16_t iq_set_buf;
int16_t Motor_Position_Control(void)
{
    int16_t speed_out;
	static uint16_t last_SC60228_Data = 0;
	static int32_t stopFlag = 0;
	static int32_t stopDetCnt = 0;

    Position_Change_Time_Count++;
    if(Position_Change_Time_Count >= Position_Change_Time)
    {
        Position_Change_Time_Count = 0;
        Ref_Index++;
			if(Ref_Index > 1) Ref_Index = 0;
			last_SC60228_Data = Phase_Ref[Ref_Index];
			if(Ref_Index ==0) iq_set_buf = 20000; else iq_set_buf = -20000;
				
			stopFlag = 0;
			stopDetCnt = 0;
		
    }
		
		if(last_SC60228_Data < SC60228_Data)
		Motor_Position_Error = (SC60228_Data - last_SC60228_Data);//%4096;
		else 
		Motor_Position_Error = (SC60228_Data + 4096 - last_SC60228_Data)%4096;
		
		last_SC60228_Data = SC60228_Data;
		
		//if(Motor_Position_Error < 20) iq_set_buf -= 200; 
		//if(Motor_Position_Error > 40) iq_set_buf += 200;
		
		
				
		if(Ref_Index == 0)
		{
			iq_set_buf += 1000;
			
			if(stopFlag == 0)
			{
				if(SC60228_Data < Phase_Ref[0])
				{
					if(++stopDetCnt > 2)
					{
						//TxByte('&');
					
						stopFlag = 1;
					}
				}else stopDetCnt = 0;
			}
		}
		else
		{
			iq_set_buf -= 1000;
			
			if(stopFlag == 0)
			{
				if(SC60228_Data > Phase_Ref[1])
				{
					if(++stopDetCnt > 2)
					{
						//TxByte('#');
					
						stopFlag = 1;
					}
				}else stopDetCnt = 0;
			}
		}
		
		if(iq_set_buf > Motor_Iq_Set_Max)
        {
            iq_set_buf = Motor_Iq_Set_Max;
        }
        else if(iq_set_buf < -Motor_Iq_Set_Max)
        {
            iq_set_buf = -Motor_Iq_Set_Max;
        }
		
		if(stopFlag) iq_set_buf = 0;
		
		//last_SC60228_Data = SC60228_Data;
		
		//log_i("%d",Motor_Position_Error);
		
		//Motor_Position_Error = (Phase_Ref[Ref_Index] + 4096 - SC60228_Data)%4096;
		
//    Motor_Position_Error = Phase_Ref[Ref_Index] - SC60228_Data;//SC60228_Data - Phase_Ref[Ref_Index];////////Phase_Ref[Ref_Index] - SC60228_Data;
//    //log_i("Phase_Ref[%u] = %u",Ref_Index,Phase_Ref[Ref_Index]);
//    if(Motor_Position_Error < -2048)
//    {
//        Motor_Position_Error += 4096;
//    }
//    else if(Motor_Position_Error > 2048)
//    {
//        Motor_Position_Error -= 4096;
//    }

	#if 0

	if(Motor_Position_Error < 50 && Motor_Position_Error > -50)
	{
		speed_out = 0;
		//TxByte('&');
	}
	else
	{
    	speed_out = Motor_Position_Error * Position_P;
		//TxByte('#');
	}
	#else
	speed_out = iq_set_buf; //Motor_Position_Error * Position_P;
	#endif

	

    return (speed_out);
}


#define RPM_SPEED_GAIN  512/60     //  电机转速转换到RPM的增益因子

static uint32_t caliTime = 0;
//static uint32_t flagCali = 1;
//static int16_t Iq_Set_Buf = 0;
void TIMER1_IRQHandler(void)	        // 4KHz中断
{
  TMR_ClearOverflowIntFlag(TIMER1);	
	
	
	Motor.PPN = POLE_NUMBER;
	
	if(M1_Flag.CaliEn)
	{
		if(SYSTEM_STATE == SYS_RUN)
		{
		Motor.Debug = 1;
		Motor.En = 1;
		Motor_Iq_Set = 0;
	
		caliTime++;
		//caliTime = 0;
		
		if(caliTime > M1_CALI_TIME)
		{
			M1_Flag.CaliEn = 0;
			caliTime = 0;
			Motor.Debug = 0;
			Motor.En = 0;
			
			Massage_SaveData();
			
			//log_w("Motor.ThetaM = %u",Motor.ThetaM);
			//log_w("Motor.ThetaE = %u",Motor.ThetaE);
			log_w("Motor.ThetaM_Offset = %u",Motor.ThetaM_Offset);
		}
		}
	}
	else
	{
		Motor.Debug = 0;
		//Motor.En = 1;
		//RPM_Set = 30;
		

    if(Motor.En && (!Motor.Debug))
    {
		if(M1_Speed_Ctrl_Mode == SPEED_LOOP)
		{
			RPM_Step = 1;
        	if(RPM_Ref < RPM_Set)
        	{
            	RPM_Ref += RPM_Step;
					if(RPM_Ref > RPM_Set) RPM_Ref = RPM_Set;
       	 	}
        	else if(RPM_Ref > RPM_Set)
        	{
        		if(RPM_Ref > RPM_Step)
        		{
            		RPM_Ref -= RPM_Step;
					if(RPM_Ref < RPM_Set) RPM_Ref = RPM_Set;
        		}
				else
				{
					RPM_Ref = RPM_Set;
				}
        	}

			Speed_Ref = RPM_Ref * RPM_SPEED_GAIN;

		
		
       	 	Motor_Iq_Set = Motor_Speed_Control(Speed_Ref, Motor.Speed, &Speed_Integer, Speed_P, Speed_I); 
					
					//Motor_Iq_Set = 6000;
		}
		else
		{
			RPM_Step = 3000;
		
			Motor_Iq_Set = Motor_Position_Control();
			

			//Motor_Iq_Set = RPM_Ref * RPM_SPEED_GAIN;
			
			/*
			if(Motor_Iq_Set < Iq_Set_Buf)
        	{
            	Motor_Iq_Set += RPM_Step;
				if(Motor_Iq_Set > Iq_Set_Buf) Motor_Iq_Set = RPM_Set;
       	 	}
        	else if(Motor_Iq_Set > Iq_Set_Buf)
        	{
        		if(Motor_Iq_Set > RPM_Step)
        		{
            		Motor_Iq_Set -= RPM_Step;
					if(Motor_Iq_Set < Iq_Set_Buf) Motor_Iq_Set = RPM_Set;
        		}
				else
				{
					Motor_Iq_Set = Iq_Set_Buf;
				}
        	}
			*/
		}

		
		
        //Speed_Ref = RPM_Ref * RPM_SPEED_GAIN;

		
		
        //Motor_Iq_Set = Motor_Speed_Control(Speed_Ref, Motor.Speed, &Speed_Integer, Speed_P, Speed_I); 
        if(Motor_Iq_Set > Motor_Iq_Set_Max)
        {
            Motor_Iq_Set = Motor_Iq_Set_Max;
        }
        else if(Motor_Iq_Set < -Motor_Iq_Set_Max)
        {
            Motor_Iq_Set = -Motor_Iq_Set_Max;
        }
    }
    else
    {
        Speed_Integer = 0;
				Position_Change_Time_Count = 20000;
    }
	}
}





